/*
@项目描述：基于ESP32-SimpleFOC的蓝牙遥控平衡小车;
@时间：2021/8/9--21:45;
@硬件：MCU-ESP32Lolin32Lite IMU-JY901 Encoder-AS5600 MotDrive-灯哥SimpleFOC powSupply-LM2596 MOT-BLDC4008;
@作者:牛童;

@还有哪里需要改进：
  硬件上面至少加个开关吧，，现在插拔太敷衍了。
  运动控制部分，转向现在用一个加上变量一个减去变量的方法不方便后续通过陀螺仪闭环以及定位，还是改成双轮差速运动学的 速度 角速度 模式好
  对小车运动方向使用陀螺仪进行闭环控制，现在小车还跑不了直线
  倾倒保护 堵转保护 失控保护
  代码太烂 全局变量很多没有必要，很多细节写的太笨
*/

#include <Arduino.h>
#include "GET_IMU.h"      //@JY901()  @GET_Angle() 
#include "FOC_Setting.h"  //@FOC_Setting()
#include "CONTROL.h"      //@Control()  @MotMove()
#include "BT_CMD.h"       //@Print_Angel()


void setup() 
{
  Serial2.begin(230400);
  Serial.begin(115200);
  SerialBT.begin("平衡小车");   //启动蓝牙，在别的设备上会配对到“平衡小车”这个蓝牙设备
  FOC_Setting();
  delay(10000);                //等一会准备准备防止上来就转夹手
}

void loop() 
{
  while(SerialBT.available()){BtCommand();}
  while(Serial2.available()){JY901();}
  GET_Angle();
  //Print_Angel();            //返回当前姿态数据//21.8.16这段程序被注释掉，因为影响程序运行速度拖慢响应周期
  Control(); 
  MotMove();
}
